#include "util.h"

#include <tiros/tiros.h>

  


#define STKSIZE             (TIROS_MIN_CTXT_SZ + 64)
#define NUM_TASKS	     3

/* A semaphore used by J1 to signal J0 */
csem_t cs;  
mutex_t m1, m2;

/* Stacks for each of our tasks */
osword_t task_stack[NUM_TASKS][STKSIZE];

void wait(void)
{
	delayms(1000);
}

void idle_task(void * dummy)
{
	while(1) {
		putstring("Idle Task running\n");
		wait();
	}
}

void assert_success(int16_t retcode, const char * msg)
{
	if (retcode != SUCCESS) {
		putstring(msg); putchar(' ');
		puthexshort(retcode); putchar('\n');
	}
}

void print_index(void)
{

	int i;
	putstring("R\tRunning - HP\n");
	putstring("P\tAchieved a P() on CS - HP\n");
	putstring("L\tLocked mutex - HP\n");
	putstring("U\tUnlocked mutex - HP\n");
	putstring("r\tRunning - LP\n");
	putstring("l\tLocked mutex - LP\n");
	putstring("u\tUnlocked mutex - LP\n");
	putstring("v\tDid a V() on CS - LP\n");
	putstring("Task\tPrio\tElev\tMsg\n");
	for(i=50;i;i--)
		putchar('-');
	putchar('\n');

	
}

void printstatus(unsigned char ch)
{
	tid_t tid;
	tid_t bp;
	tid_t ep;
	tid = os_self_tid();
	bp = os_prio_get(tid, 0);
	ep = os_prio_get(tid, O_EFFECTIVE_PRIO);

	putchar('T');
	puthexchar(tid);
	putchar('\t');
	puthexchar(bp);
	putchar('\t');
	puthexchar(ep);
	putchar('\t');
	putchar(ch);
	putchar('\n');

}


void J1(void * dummy) 
{
	int16_t rc;
	while(1) {
		wait();
		/* Lock the mutex M1 */
		printstatus('r');
		putstring("J1 locking M1\n");
		rc = mutex_lock(&m1, 0, 0);
		printstatus('l');
		assert_success(rc, "J1 lock error ");

		wait();


		/* Signal J0 to run */
		putstring("J1 signaling J0 to run\n");
		rc = csem_V(&cs);
		printstatus('v'); 
		
		wait();


		putstring("J1 locking M2\n");
		rc = mutex_lock(&m2, 0, 0);
		assert_success(rc, "J1 lock error"); 
		printstatus('l');
		putstring("J1 locked M2\n");

		wait();
		

		/* Release M2 */
		putstring("J1 unlocking M2\n");
		rc = mutex_unlock(&m2);
		assert_success(rc, "J3 unlock error ");
		printstatus('u');


		/* Release M1 */
		putstring("J1 unlocking M1\n");
		rc = mutex_unlock(&m1);
		assert_success(rc, "J3 unlock error");
		printstatus('u');

	}

}



void J0(void* dummy)
{
	int16_t rc;

	while(1) {
		putstring("J0 waiting for signal\n");
		/* Wait for a signal */
		rc = csem_P(&cs, 0, 0);
		putstring("J0 got signal\n");
		printstatus('P');


		assert_success(rc, "J0 P error");


		/* We have been signaled to wake.  Attempt a mutex lock. */
		putstring("J0 locking M2\n");
		rc = mutex_lock(&m2, 0, 0);
		assert_success(rc, "J0 lock error");
		printstatus('L');
		wait();


		putstring("J0 locking M1\n");
		rc = mutex_lock(&m1, 0, 0);
		assert_success(rc, "J0 lock error "); 

		printstatus('L');
		wait();


		putstring("J0 unlocking M1\n");
		rc = mutex_unlock(&m1);
		printstatus('U');
		assert_success(rc, "J0 unlock error ");
		wait();



		putstring("J0 unlocking M2\n");
		rc = mutex_unlock(&m2);
		printstatus('U');
		assert_success(rc, "J0 unlock error ");
		wait();

		

	}

}



void platform_init(void);
int main(void) 
{
	tid_t J0tid, J1tid, idle_tid;
	platform_init();

	// --------------------------------------------------
	os_init();

	/* We will allocate the priorities as follows: 
	 * Mutex M2 priority ceiling: 0
	 * Mutex M1 priority ceiling: 1
	 * J0 : 2
	 * J1 : 3
	 * idle_task : 4 */
	mutex_init(&m1, 1);
	mutex_init(&m2, 0);

	csem_init(&cs, 0, TIROS_CSEMVAL_MAX);


	J0tid = os_task_create(J0, (osptr_t)0,
			       task_stack[0], STKSIZE, 2); 

	J1tid = os_task_create(J1, (osptr_t)0,
			       task_stack[1], STKSIZE, 3); 

	idle_tid = os_task_create(idle_task, (osptr_t)0,
			       task_stack[2], STKSIZE, 4); 

	if (J0tid == ILLEGAL_ELEM || J1tid == ILLEGAL_ELEM ||
	    idle_tid == ILLEGAL_ELEM ) {
			putstring("Task create failed");
	}

	print_index();
	os_start();
	putstring("Started OS: Shouldn't reach here\n");
	while (1);
}

